/*
 * T_Junction_2.c
 *
 * Created: 6/11/2013 9:28:22 PM
 *  Author: Crossfire
 */

#include "T-Junction_2.h"
#include "Globals.h"

void get_Sensors2()
{
	if (PINA & (1<<3) )
	{
		senArray2[0] = 1;
	}
	else
	{
		senArray2[0]= 0;
	}

	if (PINA & (1<<4))
	{
		senArray2[1] = 1;
	}
	else
	{
		senArray2[1]=0;
	}

	if (PINA & (1<<5))
	{
		senArray2[2] = 1;
	}
	else
	{
		senArray2[2]=0;
	}
}

void set_State2()
{
	int tellerEen, tellerTwee, tellerDrie, tellerVier, tellerVijf, tellerZes, tellerZeven, tellerAcht, tellerNegen, tellerTien, tellerElf;
		int staArray2[7][4] =
		{
			{1,0,0,2},
			{0,1,0,4},
			{0,0,1,6},
			{1,1,1,4},
			{1,1,0,2},
			{0,1,1,6},
			{1,0,1,2}
		};
	if(presentState2==1)
	{
		tellerVier=1;

		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray2[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray2[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==6)
						{
							tellerElf=tellerVier;
						}


					}
				}
			}
		}
	}

	else if(presentState2==2)
	{
		tellerVier=2;
		int staArray2[7][4] =
		{
		{1,0,1,3},
		{0,1,1,3},
		{0,0,0,3},
		{0,0,1,3},
		{0,1,0,3},
		{1,1,0,3},
		{1,1,1,3},
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray2[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray2[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState2==3)
	{


		int staArray3[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray3[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray3[tellerEen][3];
					}
				}
			}
		}

		staArray2[3][3] = 4;
		staArray2[4][3] = 4;
		staArray2[6][3] = 6;

	}

	else if(presentState2==4)
	{
		tellerVier=4;
		int staArray4[7][4] =
		{
		{0,1,1,5},
		{1,0,1,5},
		{0,0,0,5},
		{0,0,1,5},
		{1,0,0,5},
		{1,1,0,5},
		{1,1,1,5},
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray4[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray4[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState2==5)
	{


		int staArray5[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray5[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray5[tellerEen][3];
					}
				}
			}
		}

		staArray2[3][3] = 6;
		staArray2[4][3] = 2;
		staArray2[5][3] = 6;

	}

	else if(presentState2==6)
	{
		tellerVier=6;
		int staArray6[7][4] =
		{
		{0,1,1,7},
		{1,1,0,7},
		{0,0,0,7},
		{0,1,0,7},
		{1,0,0,7},
		{1,0,1,7},
		{1,1,1,7}
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray6[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray6[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState2==7)
	{


		int staArray7[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray2[tellerTwee]==staArray7[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray7[tellerEen][3];
					}
				}
			}
		}

		staArray2[3][3] = 2;
		staArray2[5][3] = 4;
		staArray2[6][3] = 2;

	}


	if(tellerVier!=0 && tellerVijf==0 && tellerZes==0 && tellerZeven==0 && tellerAcht==0)
	{
		presentState2=tellerVier;
	}
	if(tellerVijf!=0)
	{
		presentState2=tellerVijf;
	}
	else if(tellerZes!=0)
	{
		presentState2=tellerZes;
	}
	else if(tellerZeven!=0)
	{
		presentState2=tellerZeven;
	}
	else if(tellerAcht!=0)
	{
		presentState2=tellerAcht;
	}
	else if(tellerNegen!=0)
	{
		presentState2=tellerNegen;
	}
	else if(tellerTien!=0)
	{
		presentState2=tellerTien;
	}
	else if(tellerElf!=0)
	{
		presentState2=tellerElf;
	}
}

void set_Trafficlights2()
{
	int stoplichten2[3];
    if(_mode == 0)
    {
        for(int i = 0; i < 3; i++)
        {
            stoplichten2[i] = _Default2[presentState2][i];
        }
    }
    else
    {
        for(int i = 0; i < 3; i++)
        {
            stoplichten2[i] = TJunction[presentState2][i];
        }
    }

	//------------------------------------------------------------------------------
	if (stoplichten2[0] == 1)
	{
		orByteC |= 0b10000000;
	}
	if (stoplichten2[0] == 2)
	{
		orByteC |= 0b01000000;
	}
	if (stoplichten2[0] == 3)
	{
		orByteC |= 0b00100000;
	}

	//------------------------------------------------------------------------------
	if (stoplichten2[1] == 1)
	{
		orByteD |=  0b00100000;
	}
	if (stoplichten2[1] == 2)
	{
		orByteD |=  0b01000000;
	}
	if (stoplichten2[1] == 3)
	{
		orByteD |=  0b10000000;
	}

	//-------------------------------------------------------------------------------
	if (stoplichten2[2] == 1)
	{
		orByteD |=  0b00000100;
	}
	if (stoplichten2[2] == 2)
	{
		orByteD |=  0b00001000;
	}
	if (stoplichten2[2] == 3)
	{
		orByteD |=  0b00010000;
	}
}

void set_Delay2()
{
	if(orByteC == 0b00110000 || orByteC == 0b00100100 || orByteD == 0b10000100 || orByteD== 0b00110000)
	{
		vertraging2 = 4;
	}
	else if(orByteC == 0b01010000 || orByteC == 0b01000100 || orByteD == 0b00101000 || orByteD == 0b01000100)
	{
		vertraging2 = 2;
	}
	else
	{
		vertraging2 = 1;
	}
}
